//
// Created by YangShuai on 2022/12/11.
//

/**
  ******************************************************************************
  * @file    driver_remote.c
  * @author  XJTU ROBOMASTER Team
  * @brief   remote driver
  *          这个文件提供函数对遥控器数据进行处理得到拨杆和键鼠控制量，对机器人控制信号进行更新
  *
  @verbatim
  ==============================================================================
                        ##### How to use this driver #####
  ==============================================================================
    [..]
      (#) Update the remote data by implementing the RC_Update():
          (++) Update the remote parameters, including:
		       (+++) RC_Ctl

      (#) Update the remote data by implementing the RemoteControl():
          (++) Update the remote parameters, including:
		       (+++) RemoteData

  @endverbatim
  ******************************************************************************
  * @attention
  *
  * 函数 RC_Update() 已经封装好，遥杆和键鼠的数据接收均为正确的不需要改
  * 可以根据需要更改结构体 RemoteData_t 中的成员变量，并更改函数 RemoteControl()
  *
  * Please add comments after adding or deleting functions to ensure code
  * specification.
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "driver_remote.h"
#include "math.h"

Remote_message_t remote_message;

void Remote_DMA_enable()
{
    remote_message.huart = &huart1;
    HAL_UARTEx_ReceiveToIdle_DMA(remote_message.huart, remote_message.remote_data_raw.RemoteDataRaw, RC_FRAME_LENGTH);
    __HAL_DMA_DISABLE_IT(remote_message.huart->hdmarx, DMA_IT_HT);
}

//0,正常
//1，数据有误，不要用
uint8_t JudgeDataValidity(uint16_t data)
{
    if(data<=1685&&data>=363)
        return 0;
    else
        return 1;
}

//0,正常
//1，数据有误，不要用
RemoteDataProcessed_t RemoteDataProcess(RemoteDataRaw_Union RemoteDataRaw)
{
    RemoteDataProcessed_t RemoteDataProcessed;

    uint16_t Ch2=0;
    uint8_t JudgeDataValiditySum=0;
    float MouseX,MouseY,MouseZ;
//**********************************摇杆***********************************************
    Ch2 = RemoteDataRaw.RemoteDataProcessed.RCValue.Ch2_l | RemoteDataRaw.RemoteDataProcessed.RCValue.Ch2_h<<2;

    JudgeDataValiditySum = JudgeDataValidity(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch0) + JudgeDataValidity(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch1) + JudgeDataValidity(Ch2) +JudgeDataValidity(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch3);

    if(JudgeDataValiditySum)					//检查数据合法性
    {
        RemoteDataProcessed.FlagValidity=0;
        //FlagValidity=0;
        return RemoteDataProcessed;
    }

//	FlagValidity=1;

    //基本量偏移消除
    if(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch0>1030||RemoteDataRaw.RemoteDataProcessed.RCValue.Ch0<1018)
        RemoteDataProcessed.Channel_0 =  ((float)(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch0-RC_CH_VALUE_OFFSET))/(RC_CH_VALUE_MAX-RC_CH_VALUE_OFFSET);
    else
        RemoteDataProcessed.Channel_0 = 0;

    //基本量偏移消除
    if(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch1>1030||RemoteDataRaw.RemoteDataProcessed.RCValue.Ch1<1018)
        RemoteDataProcessed.Channel_1 =  ((float)(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch1-RC_CH_VALUE_OFFSET))/(RC_CH_VALUE_MAX-RC_CH_VALUE_OFFSET);
    else
        RemoteDataProcessed.Channel_1 = 0;

    if(Ch2>1030||Ch2<1018)
        RemoteDataProcessed.Channel_2 =  ((float )(Ch2-RC_CH_VALUE_OFFSET))/(RC_CH_VALUE_MAX-RC_CH_VALUE_OFFSET);
    else
        RemoteDataProcessed.Channel_2 = 0;

    if(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch3>1030||RemoteDataRaw.RemoteDataProcessed.RCValue.Ch3<1018)
        RemoteDataProcessed.Channel_3 =  ((float)(RemoteDataRaw.RemoteDataProcessed.RCValue.Ch3-RC_CH_VALUE_OFFSET))/(RC_CH_VALUE_MAX-RC_CH_VALUE_OFFSET);
    else
        RemoteDataProcessed.Channel_3 = 0;

//**********************************拨扭***********************************************
    RemoteDataProcessed.LeftSwitch = (SwitchEnum)(RemoteDataRaw.RemoteDataProcessed.RCValue.s2);
    RemoteDataProcessed.RightSwitch= (SwitchEnum)(RemoteDataRaw.RemoteDataProcessed.RCValue.s1);

//**********************************鼠标***********************************************
    MouseX = RemoteDataRaw.RemoteDataProcessed.Mouse.x / 32768.0f;
    MouseY = RemoteDataRaw.RemoteDataProcessed.Mouse.y / 32768.0f;
    MouseZ = RemoteDataRaw.RemoteDataProcessed.Mouse.z / 32768.0f;

    if(MouseX>=MOUSEERRORTOLERANCE||MouseX<=-MOUSEERRORTOLERANCE)
        RemoteDataProcessed.MouseX	=	MouseX;
    else
        RemoteDataProcessed.MouseX	=	0;

    if(MouseY>=MOUSEERRORTOLERANCE||MouseY<=-MOUSEERRORTOLERANCE)
        RemoteDataProcessed.MouseY	=	MouseY;
    else
        RemoteDataProcessed.MouseY	=	0;

    if(MouseZ>=MOUSEERRORTOLERANCE||MouseZ<=-MOUSEERRORTOLERANCE)
        RemoteDataProcessed.MouseZ	=	MouseZ;
    else
        RemoteDataProcessed.MouseZ	=	0;

    RemoteDataProcessed.LeftMousePress	=	(MousePressEnum)(RemoteDataRaw.RemoteDataProcessed.Mouse.Press_l);
    RemoteDataProcessed.RightMousePress	=	(MousePressEnum)(RemoteDataRaw.RemoteDataProcessed.Mouse.Press_r);

    RemoteDataProcessed.KeyW	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyW;
    RemoteDataProcessed.KeyS	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyS;
    RemoteDataProcessed.KeyA	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyA;
    RemoteDataProcessed.KeyD	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyD;
    RemoteDataProcessed.KeyShift=	RemoteDataRaw.RemoteDataProcessed.Key.KeyShift;
    RemoteDataProcessed.KeyCtrl	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyCtrl;
    RemoteDataProcessed.KeyQ	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyQ;
    RemoteDataProcessed.KeyE	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyE;
    RemoteDataProcessed.KeyR	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyR;
    RemoteDataProcessed.KeyF	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyF;
    RemoteDataProcessed.KeyG	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyG;
    RemoteDataProcessed.KeyZ	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyZ;
    RemoteDataProcessed.KeyX	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyX;
    RemoteDataProcessed.KeyC	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyC;
    RemoteDataProcessed.KeyV	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyV;
    RemoteDataProcessed.KeyB	=	RemoteDataRaw.RemoteDataProcessed.Key.KeyB;

    RemoteDataProcessed.FlagValidity=1;
    return RemoteDataProcessed;
}

void RockerDataConvert(float *x,float *y)
{
    if(fabs(*x)+fabs(*y)>1)
    {
        if(*y>0 && *x>0)				//1象限
        {
            *y=*y/(*y+*x);
            *x=*x/(*y+*x);
        }
        else if(*y>0 && *x<0)		//2象限
        {
            *x=*x/(*y-*x);
            *y=*y/(*y-*x);
        }
        else if(*y<0 && *x>0)		//4象限
        {
            *x=-*x/(*y-*x);
            *y=-*y/(*y-*x);
        }
        else										//3象限
        {
            *x=-*x/(*x+*y);
            *y=-*y/(*x+*y);
        }
    }
}


RemoteData_t RemoteDataCalculate(RemoteDataProcessed_t	RemoteDataReceive)
{
    RemoteData_t	RemoteDataPortTemp={0};
    if(remote_message.remote_data_raw.RemoteDataProcessed.RCValue.s2)
    {
        // 遥控器收到数据 ————> 控制底盘运动、云台转动
        RemoteDataPortTemp.ChassisSpeedX	=	 	RemoteDataReceive.Channel_2;
        RemoteDataPortTemp.ChassisSpeedY	=	 	RemoteDataReceive.Channel_3;

        RemoteDataPortTemp.PitchIncrement	=		RemoteDataReceive.Channel_1;
        RemoteDataPortTemp.YawIncrement		=	 	RemoteDataReceive.Channel_0;

        RemoteDataPortTemp.s1=RemoteDataReceive.LeftSwitch;
        RemoteDataPortTemp.s2=RemoteDataReceive.RightSwitch;

        RockerDataConvert(&(RemoteDataPortTemp.ChassisSpeedX),&(RemoteDataPortTemp.ChassisSpeedY));
        // Rocker是什么 ————> 鼠标 √
    }

    switch(RemoteDataReceive.RightSwitch)
    {
        case SwitchUp:
            RemoteDataPortTemp.Friction=DISABLE;
            RemoteDataPortTemp.FeedMotor=DISABLE;
            break;
        case SwitchMid:
            RemoteDataPortTemp.Friction=ENABLE;
            RemoteDataPortTemp.FeedMotor=DISABLE;
            break;
        case SwitchDown:
            RemoteDataPortTemp.Friction=ENABLE;
            RemoteDataPortTemp.FeedMotor=ENABLE;
            break;
        default:
            break;
    }
    RemoteDataPortTemp.Laser=RemoteDataPortTemp.Friction;

    return RemoteDataPortTemp;
}

//没用这个更新，现在的更新是在driver_communicate.c中
void update_chassisControlData(ChassisControlDataStruct  *chassisControlData)
{
    switch (remote_message.remote_data.s1) {
        case SwitchUp:
            chassisControlData->chassisMode = CHASSIS_MODE_GYRO;
            chassisControlData->speedX = remote_message.remote_data.ChassisSpeedX;
            chassisControlData->speedY = (-remote_message.remote_data.ChassisSpeedY);
            chassisControlData->speedSpin = remote_message.remote_data.YawIncrement * 1.2f;
//            chassisControlData->superFlag = remote_message.remote_data.Friction;
            break;

        case SwitchMid:
            chassisControlData->chassisMode = CHASSIS_MODE_FOLLOW;
            chassisControlData->speedX = remote_message.remote_data.ChassisSpeedX;
            chassisControlData->speedY = (-remote_message.remote_data.ChassisSpeedY) * 0.01f + chassisControlData->lastspeedY * 0.99f;
            chassisControlData->speedSpin = remote_message.remote_data.YawIncrement * 1.2f;
//            chassisControlData->superFlag = remote_message.remote_data.Friction;
            chassisControlData->lastspeedY = chassisControlData->speedY;
            break;

        case SwitchDown:
            break;

        default:
            chassisControlData->speedX = 0.f;
            chassisControlData->speedY = 0.f;
            chassisControlData->speedSpin = 0.f;
            chassisControlData->superFlag = 0;
            break;
    }
}

uint8_t RemoteTaskControl()
{
    //Step	1	:	Receive remote raw data from buffer
    remote_message.remote_data_processed = RemoteDataProcess(remote_message.remote_data_raw);

    //Step	2	:	Judge Validity
    if(remote_message.remote_data_processed.FlagValidity)
    {
        //Step	3	：Process	remote data	and	Save into remote_data
        remote_message.remote_data = RemoteDataCalculate(remote_message.remote_data_processed);
        return 0;
    }

    return 1;
}
/************************ (C) COPYRIGHT XJTU ROBOMASTER ********END OF FILE****/
